package Model.SurroundModel;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.ArrayList;

public class ShipSampling {
    public ArrayList<ShipSampling> ships = new ArrayList<>(); //船队
    public static double PI = Math.PI;
    public int num; //船只编号
    public double v; //前进速度
    public double u; //角速度
    public double θ; //智能体和目标中心的连线与x的夹角
    public double l; //半径
    public double ρ; //当前小船对目标距离目标平均中心的最大估计值
    public double φx = 0;
    public double φy = 0;//表示目标平均中心动态估计器的内部状态
    public double x; //船的x坐标
    public double y; //船的y坐标 x,y用于y（t）
    public double x0; //船的x坐标
    public double y0; //船的y坐标 x0，y0用于计算r（t）
    public double xx; //船的x坐标
    public double yy; //船的y坐标 xx,yy用于计算p（t）p(t)是目标中心的位置
    public double ux; //船的x坐标
    public double uy; //船的y坐标 xx,yy用于计算p（t）p(t)是目标中心的位置
    public double el;
    public double eθ;

    public ShipSampling() {
    }
    public ShipSampling(int num, double x, double y) {
        this.x = x;
        this.y = y;
        this.num = num;
    }

    //计算欧氏距离
    public  double distance(double x1, double y1, double x2, double y2) {
        return Math.sqrt(Math.pow((x1 - x2), 2) + Math.pow((y1 - y2), 2));
    }

    //根据采样的虚拟目标的点来创立船只
    public void createShip(int n , Target target){
        for (int i = 0 ; i<n;i++){
            this.ships.add(new ShipSampling(i+1,0 ,0));
        }
    }
    //  接下来进行p（t）和 φ（t） 的计算
    public  void setP(double t,Target Target) {
        for (int pi = 0 ;pi<Target.targets.size();pi++){
            ships.get(pi).xx = Target.targets.get(ships.get(pi).num - 1).x + ships.get(pi).φx;
            ships.get(pi).yy = Target.targets.get(ships.get(pi).num - 1).y + ships.get(pi).φy;
            BigDecimal B = new BigDecimal(ships.get(pi).xx);
            ships.get(pi).xx = B.setScale(3, RoundingMode.HALF_UP).doubleValue();
            BigDecimal C = new BigDecimal(ships.get(pi).yy);
            ships.get(pi).yy = C.setScale(3, RoundingMode.HALF_UP).doubleValue();
            Target.targets.get(ships.get(pi).num - 1).xx = ships.get(pi).xx;
            Target.targets.get(ships.get(pi).num - 1).yy = ships.get(pi).yy;
        }
        for (int pii = Target.targets.size();pii< ships.size();pii++){
            ships.get(pii).xx=ships.get(pii-1).xx;
            ships.get(pii).yy=ships.get(pii-1).yy;
        }
    }

    public  void setφ(double t) {
        double φ1 = 0;
        double φ2 = 0;
        for (ShipSampling ship : this.ships) {
            φ1 = 0;
            φ2 = 0;
            if (ship.num == 1) {
                φ1 = φ1 + Math.signum(ships.get(ship.num).xx - ship.xx);
                φ2 = φ2 + Math.signum(ships.get(ship.num).yy - ship.yy);
            } else if(ship.num == ships.size()){
                φ1 = φ1 + Math.signum(ships.get(ship.num - 2).xx - ship.xx);
                φ2 = φ2 + Math.signum(ships.get(ship.num - 2).yy - ship.yy);
            }else{
                φ1 = φ1 + Math.signum(ships.get(ship.num - 2).xx - ship.xx) + Math.signum(ships.get(ship.num).xx - ship.xx);
                φ2 = φ2 + Math.signum(ships.get(ship.num - 2).yy - ship.yy) + Math.signum(ships.get(ship.num).yy - ship.yy);
            }
            φ1 = Parameter.alpha * t * φ1;
            φ2 = Parameter.alpha * t * φ2;
            ship.φx = φ1 + ship.φx;
            ship.φy = φ2 + ship.φy;
        }
    }

    //目标队列包围控制器
    public  void formationCotroller(double t,Target Target,double[][] A) {
        double x = 0;
        double y = 0;
        int i = 0;
        int j = 0;

        setφ(t);
        setP(t,Target);

        for (ShipSampling ship : ships) {
            i = ship.num - 1;
            ship.x0 = -FormationParameter.k0 * ((ship.x - ship.xx) - FormationParameter.k1 * (Target.virtualTargets.get(i).x - ship.xx));
            ship.y0 = -FormationParameter.k0 * ((ship.y - ship.yy) - FormationParameter.k1 * (Target.virtualTargets.get(i).y - ship.yy));
            //System.out.println("ship.num:" + ship.num + "  ship.xx:" + ship.xx + "  ship.yy:" + ship.yy);
            for (ShipSampling shipx : ships) {
                j = ship.num - 1;
                x += A[i][j] * ((shipx.x - shipx.xx - FormationParameter.k1 * (Target.virtualTargets.get(j).x - shipx.xx))
                        - (ship.x - ship.xx - FormationParameter.k1 * (Target.virtualTargets.get(i).x - ship.xx)));
                y += A[i][j] * ((shipx.y - shipx.yy - FormationParameter.k1 * (Target.virtualTargets.get(j).y - shipx.yy))
                        - (ship.y - ship.yy - FormationParameter.k1 * (Target.virtualTargets.get(i).y - ship.yy)));

            }
            ship.ux = ship.x0 + x*t;
            ship.uy = ship.y0 + y*t;
            //System.out.println("ship.num:"+ship.num+ "  ship.ux:" + ship.ux + "  ship.uy:" + ship.uy);
            x = 0;
            y = 0;
            ship.x += ship.ux * t;
            ship.y += ship.uy * t;
        }

    }

    //判断停止的函数
    public  boolean circularFinish(double t,Ship Ship,Target Target) {
        double max = 0;
        double e = 0;
        int jud = 4;
        for (int i = 0; i < Parameter.N; i++) {
            if (distance(Target.targets.get(i).x, Target.targets.get(i).y, Ship.ships.get(i).xx, Ship.ships.get(i).yy) > max)
                max = distance(Target.targets.get(i).x, Target.targets.get(i).y, Ship.ships.get(i).xx, Ship.ships.get(i).yy);
        }
        for (ShipSampling ship : ships) {
            ship.el = distance(ship.x, ship.y, ship.xx,ship.yy) - Parameter.k * max;
            ship.eθ = 1;
            for (ShipSampling shipx : ships) {
                e = ship.θ - shipx.θ - 2 * Math.PI * (shipx.num - ship.num) / Parameter.N;
                if (e == 0)
                    jud--;
            }
            if (jud == 0)
                ship.eθ = 0;
            jud = 4;
        }
        for (ShipSampling ship : ships) {
            if (Math.abs(ship.el) >= 0.01 && Math.abs(ship.eθ) == 0) {
                jud--;
            }
        }
        if (jud <= 0) {
            return true;
        } else
            return false;
    }

    //判断目标队列停止的函数
    public  boolean formationFinish(double t,Target Target) {
        double e = 0;
        int i = 0;
        int jud = 4;
        double x1 = 0;
        double x2 = 0;
        double y1 = 0;
        double y2 = 0;
        Target.setCenter();

        for (ShipSampling ship : ships) {
            i = ship.num - 1;
            x1 = ship.x - Target.x0;
            y1 = ship.y - Target.y0;
            x2 = FormationParameter.k * (Target.virtualTargets.get(i).x - Target.x0);
            y2 = FormationParameter.k * (Target.virtualTargets.get(i).y - Target.y0);
            e = distance(x1, y1, x2, y2);
            if (Math.abs(e) == 0)
                jud--;
        }
        if (jud <= 0) {
            return true;
        } else {
            return false;
        }
    }
}
